#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "pcl_conversions/pcl_conversions.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl/io/pcd_io.h"
#include <sstream>
#include <vector>
#include "mqtt/async_client.h"

// MQTT配置
const std::string MQTT_SERVER_ADDRESS = "tcp://10.255.0.60:1883";
const std::string MQTT_CLIENT_ID = "ros2_pointcloud_sender";
const std::string MQTT_TOPIC = "pointcloud/data";

class MqttPointCloudSender : public rclcpp::Node
{
public:
    MqttPointCloudSender() : Node("mqtt_pointcloud_sender")
    {
        // 创建点云订阅者
        subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "input_pointcloud", 10,
            std::bind(&MqttPointCloudSender::pointcloud_callback, this, std::placeholders::_1));

        // 初始化MQTT客户端
        mqtt_client_ = std::make_unique<mqtt::async_client>(MQTT_SERVER_ADDRESS, MQTT_CLIENT_ID);
        
        // 设置连接选项
        mqtt::connect_options connOpts;
        connOpts.set_keep_alive_interval(20);
        connOpts.set_clean_session(true);

        // 连接MQTT服务器
        try {
            RCLCPP_INFO(this->get_logger(), "Connecting to MQTT server: %s", MQTT_SERVER_ADDRESS.c_str());
            auto connToken = mqtt_client_->connect(connOpts);
            connToken->wait();
            RCLCPP_INFO(this->get_logger(), "Connected to MQTT server");
        }
        catch (const mqtt::exception& e) {
            RCLCPP_ERROR(this->get_logger(), "MQTT connection failed: %s", e.what());
            rclcpp::shutdown();
        }
    }

    ~MqttPointCloudSender()
    {
        // 断开MQTT连接
        if (mqtt_client_->is_connected()) {
            mqtt_client_->disconnect()->wait();
            RCLCPP_INFO(this->get_logger(), "Disconnected from MQTT server");
        }
    }

private:
    void pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
        if (!mqtt_client_->is_connected()) {
            RCLCPP_WARN(this->get_logger(), "Not connected to MQTT server, skipping pointcloud");
            return;
        }

        try {
            // 将ROS消息转换为PCL点云
            pcl::PointCloud<pcl::PointXYZ> cloud;
            pcl::fromROSMsg(*msg, cloud);

            // 序列化点云数据
            std::stringstream ss;
            pcl::PCDWriter writer;
            writer.writeBinaryCompressed<pcl::PointXYZ>(ss, cloud);
            std::string data = ss.str();

            // 发送MQTT消息
            mqtt::message_ptr pubmsg = mqtt::make_message(MQTT_TOPIC, data);
            pubmsg->set_qos(0);  // QoS 0适合点云这种高频数据
            mqtt_client_->publish(pubmsg)->wait_for(std::chrono::seconds(1));

            RCLCPP_INFO(this->get_logger(), "Sent pointcloud with %zu points", cloud.size());
        }
        catch (const std::exception& e) {
            RCLCPP_ERROR(this->get_logger(), "Error processing pointcloud: %s", e.what());
        }
    }

    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
    std::unique_ptr<mqtt::async_client> mqtt_client_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MqttPointCloudSender>());
    rclcpp::shutdown();
    return 0;
}
